﻿#include "robotsimulatewidget.h"
#include <QtWidgets>
#include "osgwidget.h"
#include "simulatecontrolwidget.h"
#include "global.h"                 /// Axis
#include "ifkinematicshandle.h"     /// d1 a1 a2 a3 d4
#include "setrobotparameterdialog.h"
#include "teachwidget.h"        /// 示教界面
#include "customobjectdialog.h"

using namespace std;


RobotSimulateWidget::RobotSimulateWidget(QWidget *parent) : QWidget(parent),
    fileName(""),
    ifkinematicsHandle(IFKinematicsHandle::getInstance()),
    textEdit(QSharedPointer<QTextEdit>(new QTextEdit(this))),
    simulateControlWidget(QSharedPointer<SimulateControlWidget>(SimulateControlWidget::createSimulateControlWidget(this))),
    osgWidget(QSharedPointer<OsgWidget>(new OsgWidget)),
    gridLayout(QSharedPointer<QGridLayout>(new QGridLayout(this))),
    teachWidget(QSharedPointer<TeachWidget>(new TeachWidget(this))),
    dialog(nullptr),
    customObjectDialog(nullptr)
{
#if 0
    pFunc jointRotate[] = {&RobotSimulateWidget::BaseRotate,
                           &RobotSimulateWidget::J1Rotate,
                           &RobotSimulateWidget::J2Rotate,
                           &RobotSimulateWidget::J3Rotate,
                           &RobotSimulateWidget::J4Rotate,
                           &RobotSimulateWidget::J5Rotate,
                           &RobotSimulateWidget::J6Rotate,};
#endif
    init();
    layout();
    createAction();
}


RobotSimulateWidget::~RobotSimulateWidget()
{
    fileName = "";
}


/*初始化仿真界面内部的控件进行配置
 * 设置diplayWindow背景色
 * 设置osgWidget背景色
 * 设置simulateControlWidget背景色
*/
void RobotSimulateWidget::init()
{

    osgWidget->setAutoFillBackground(true);              // 必须设置运行时自动填冲
    QPalette palette1 = osgWidget->palette();            // 获取容器的调色面板,设置成绿色
    palette1.setColor(QPalette::Window, Qt::black);      // 设置背景调色板颜色
    osgWidget->setPalette(palette1);

    teachWidget->setAutoFillBackground(true);
    QPalette palette2 = teachWidget->palette();
    palette2.setColor(QPalette::Window, Qt::white);
    teachWidget->setPalette(palette2);

    simulateControlWidget->setAutoFillBackground(true);
    QPalette controlWidgetPalette = simulateControlWidget->palette();
    controlWidgetPalette.setColor(QPalette::Window, Qt::white);
    simulateControlWidget->setPalette(controlWidgetPalette);

    /// 定义弹窗
    dialog = QSharedPointer<SetRobotParameterDialog>(new SetRobotParameterDialog(this));
    customObjectDialog = QSharedPointer<CustomObjectDialog>(new CustomObjectDialog(this));


    /// 设置模态
    dialog->setModal(false);
    customObjectDialog->setModal(false);


}


/*设计仿真界面内各部件的布局
 * @osgWidget机器人渲染界面的控件
 * @textEdit文本编辑界面
 * @drawPathWidget绘制轨迹界面
 * @simulateControlWidget机器人控制界面
*/
void RobotSimulateWidget::layout()
{
    gridLayout->addWidget(osgWidget.data(), 0, 0, 1, 1);
    gridLayout->addWidget(simulateControlWidget.data(), 0, 1, 1, 1);
    gridLayout->addWidget(textEdit.data(), 1, 0, 1, 1);
    gridLayout->addWidget(teachWidget.data(), 1, 1, 1, 1);

    // 第一行高:第二行高 = 3:1 第一列宽:第二列宽 = 4:1
    gridLayout->setColumnStretch(0, 4);
    gridLayout->setColumnStretch(1, 1);
    gridLayout->setRowStretch(0, 3);
    gridLayout->setRowStretch(1, 1);

    //设置水平、垂直、以及边缘边距为5
    gridLayout->setHorizontalSpacing(10);
    gridLayout->setVerticalSpacing(10);
    gridLayout->setContentsMargins(10, 10, 10, 10);
    setLayout(gridLayout.data());
}


/* 连接信号与槽 */
void RobotSimulateWidget::createAction() const
{

    // 连接控制台listwidget的所选item下标和渲染界面节点迭代器下标
    connect(simulateControlWidget->getJointListWidet(), SIGNAL(currentTextChanged(QString)), osgWidget.data(), SLOT(setCurrentJointText(QString)));
    connect(simulateControlWidget->getObjectListWidet(), SIGNAL(currentTextChanged(QString)), osgWidget.data(), SLOT(setCurrentObjectText(QString)));

    /// 连接到机器人运动学句柄上 和 控制台
    connect(dialog.data(), SIGNAL(sendParameter(QVector<float>)), IFKinematicsHandle::getInstance(), SLOT(recvParameters(QVector<float>)));
    connect(dialog.data(), SIGNAL(sendParameter(QVector<float>)), this->simulateControlWidget.data(), SLOT(recvParameters(QVector<float>)));

    connect(customObjectDialog.data(), SIGNAL(buildBox(std::vector<float>)), this, SLOT(buildBox(std::vector<float>)));
    connect(customObjectDialog.data(), SIGNAL(buildBall(float)), this, SLOT(buildBall(float)));
    connect(customObjectDialog.data(), SIGNAL(buildCylinder(std::vector<float>)), this, SLOT(buildCylinder(std::vector<float>)));

    connect(teachWidget.data(), SIGNAL(moveJButton()), this, SLOT(addMoveLinePath()));
    connect(teachWidget.data(), SIGNAL(moveLineButton()), this, SLOT(addLinePath()));
    connect(teachWidget.data(), SIGNAL(moveArcButton()), this, SLOT(addArcPath()));
    connect(teachWidget.data(), SIGNAL(runButton()), this, SLOT(run()));
    connect(teachWidget.data(), SIGNAL(clearAllButton()), this, SLOT(clearPathArray()));
    connect(teachWidget.data(), SIGNAL(shadowOpen()), this->osgWidget.data(), SLOT(drawOneShadow()));

}


/** 添加空移线的轨迹点 */
void RobotSimulateWidget::addMoveLinePath()
{

#ifndef NDEBUG
    qDebug() << "RobotSimulateWidget::addMoveLinePath()";
#endif

    /// 获取当前各关节角度值z作为终点
    Joint endJoint = simulateControlWidget->getCurJointsAngle();

    /// 当第一条轨迹加入时 定义成一条长度为0,起点和终点重合的轨迹 作为整体轨迹的起点
    Joint beginJoint;

    if (!pathElementArray.empty()) {
        /// 获取上一个轨迹点的关节角度值作为起点
        beginJoint = pathElementArray.back().getEndJoint();
    } else {
        beginJoint = endJoint;
    }

    /// 构造轨迹并插入轨迹数组中
    PathElement elem(beginJoint, endJoint, MoveJ);
    pathElementArray.push_back(elem);

#if 0   /// 绘制一个点
    Point end = elem.getEndPoint();
    this->osgWidget->drawPoint({end.x, end.y, end.z});
    qDebug() << end.x << end.y << end.z;
#endif

    this->teachWidget->addItem(QString(QStringLiteral("空移直线")));

    displayOnTextEdit(QString(QStringLiteral("添加空移线轨迹点%1")).arg(pathElementArray.size()));

}


static bool isAddMidPoint = true;

/** 添加直线线的轨迹点 */
void RobotSimulateWidget::addLinePath()
{

#ifndef NDEBUG
    qDebug() << "RobotSimulateWidget::addLinePath()";
#endif

    if (isAddMidPoint == false) {
        displayOnTextEdit(QString(QStringLiteral("请先添加完圆弧的终点")));
        return;
    }

    /// 获取当前各关节角度值作为终点
    Joint endJoint = simulateControlWidget->getCurJointsAngle();

    /// 当第一条轨迹加入时 定义成一条长度为0,起点和终点重合的轨迹 作为整体轨迹的起点
    Joint beginJoint;

    if (!pathElementArray.empty()) {
        /// 获取上一个轨迹点的关节角度值作为起点
        beginJoint = pathElementArray.back().getEndJoint();

    } else {
        displayOnTextEdit(QString(QStringLiteral("直线不能作为起点 请先添加空移直线")));
        return;
    }

    /// 构造轨迹并插入轨迹数组中
    PathElement elem(beginJoint, endJoint, MoveLine);
    pathElementArray.push_back(elem);

#if 0   /// 绘制一个点
    Point end = elem.getEndPoint();
    this->osgWidget->drawPoint({end.x, end.y, end.z});
#endif

    this->teachWidget->addItem(QString(QStringLiteral("直线")));

    /// 获取起点和终点
    Point beginPoint = elem.getBeginPoint();
    Point endPoint = elem.getEndPoint();


    displayOnTextEdit(QString(QStringLiteral("添加直线轨迹线%1 起点: (%2 %3 %4) 终点: (%5, %6, %7)"))
                      .arg(pathElementArray.size()).arg(beginPoint.x).arg(beginPoint.y).arg(beginPoint.z)
                      .arg(endPoint.x).arg(endPoint.y).arg(endPoint.z));

}



/** 添加圆弧线的轨迹点 */
void RobotSimulateWidget::addArcPath()
{

#ifndef NDEBUG
    qDebug() << "RobotSimulateWidget::addArcPath()";
#endif

    static Joint midJoint = Joint();
    static Joint endJoint = Joint();

    if (isAddMidPoint) {
        /// 获取当前各关节角度值作为终点
        midJoint = simulateControlWidget->getCurJointsAngle();
        isAddMidPoint = false;
        displayOnTextEdit(QString(QStringLiteral("添加圆弧中间点")));

        return;

    } else {
        /// 获取当前各关节角度值作为终点
        endJoint = simulateControlWidget->getCurJointsAngle();
        isAddMidPoint = true;
    }


    /// 当第一条轨迹加入时 定义成一条长度为0,起点和终点重合的轨迹 作为整体轨迹的起点
    Joint beginJoint;

    if (!pathElementArray.empty()) {
        /// 获取上一个轨迹点的关节角度值作为起点
        beginJoint = pathElementArray.back().getEndJoint();

    } else {
        displayOnTextEdit(QString(QStringLiteral("圆弧不能作为起点 请先添加空移直线")));
        return;
    }

    /// 构造轨迹并插入轨迹数组中
    PathElement elem(beginJoint, midJoint, endJoint, MoveArc);
    pathElementArray.push_back(elem);

#if 1   /// 绘制一个点
    Point end = elem.getEndPoint();
    Point mid = elem.getMidPoint();
    this->osgWidget->drawPoint({mid.x, mid.y, mid.z});
    this->osgWidget->drawPoint({end.x, end.y, end.z});
#endif

    this->teachWidget->addItem(QString(QStringLiteral("圆弧")));

    /// 获取起点和终点
    Point beginPoint = elem.getBeginPoint();
    Point midPoint = elem.getMidPoint();
    Point endPoint = elem.getEndPoint();


    displayOnTextEdit(QString(QStringLiteral("添加圆弧轨迹线%1 起点: (%2 %3 %4) 中点: (%5 %6 %7) 终点: (%8 %9 %10)"))
                      .arg(pathElementArray.size())
                      .arg(beginPoint.x).arg(beginPoint.y).arg(beginPoint.z)
                      .arg(midPoint.x).arg(midPoint.y).arg(midPoint.z)
                      .arg(endPoint.x).arg(endPoint.y).arg(endPoint.z));

}



void RobotSimulateWidget::run()
{

#ifndef NDEBUG
    qDebug() << "RobotSimulateWidget::run()";
#endif

    /// 如果轨迹数组为空则返回并输出日志
    if (pathElementArray.empty()) {
        displayOnTextEdit(QStringLiteral("请添加轨迹"));
        return;

    }

    /// 开始运行
    displayOnTextEdit(QStringLiteral("==============[开始运行]================="));

    /// 获取当前各关节角度值
    Joint curJ = simulateControlWidget->getCurJointsAngle();
    /// 将当前位置作为起点位置,逆解时需要
    lastJoint = curJ;

    /// 从当前位置先运动到第一条轨迹的起点
    PathElement& firstElement = pathElementArray[0];
    this->osgWidget->moveJ(curJ, firstElement.getBeginJoint());

    /// 遍历整个轨迹数组
    for (PathElement & element : pathElementArray) {

        /// 如果轨迹是空移直线
        if (element.getPathElementType() == MoveJ) {

            displayOnTextEdit(QString(QStringLiteral("------[空移直线]------")));
            Joint beginJoint = element.getBeginJoint();
            Joint endJoint = element.getEndJoint();
            Point begin = element.getBeginPoint();
            Point end = element.getEndPoint();
            displayOnTextEdit(QString(QStringLiteral("起点坐标:(%1, %2, %3)")).arg(begin.x).arg(begin.y).arg(begin.z));
            displayOnTextEdit(QString(QStringLiteral("终点坐标:(%1, %2, %3)")).arg(end.x).arg(end.y).arg(end.z));
            displayOnTextEdit(QString(QStringLiteral("起点关节角:(%1, %2, %3, %4, %5, %6)")).arg(beginJoint.j1).arg(beginJoint.j2).arg(beginJoint.j3).arg(beginJoint.j4).arg(beginJoint.j5).arg(beginJoint.j6));
            displayOnTextEdit(QString(QStringLiteral("终点关节角:(%1, %2, %3, %4, %5, %6)")).arg(endJoint.j1).arg(endJoint.j2).arg(endJoint.j3).arg(endJoint.j4).arg(endJoint.j5).arg(endJoint.j6));
            /// 起点处绘制一个阴影
            this->osgWidget->drawOneShadow();
            /// 开始移动机器人
            this->osgWidget->moveJ(beginJoint, endJoint);

        /// 如果轨迹是直线
        } else if (element.getPathElementType() == MoveLine) {

            displayOnTextEdit(QString(QStringLiteral("--------[直线]--------")));
            Point begin = element.getBeginPoint();
            Point end = element.getEndPoint();
            displayOnTextEdit(QString(QStringLiteral("起点坐标:(%1, %2, %3)")).arg(begin.x).arg(begin.y).arg(begin.z));
            displayOnTextEdit(QString(QStringLiteral("终点坐标:(%1, %2, %3)")).arg(end.x).arg(end.y).arg(end.z));
            /// 直线轨迹开始前先绘制一个阴影
            this->osgWidget->drawOneShadow();
            /// 插补获取各轴角度数组
            std::vector<Joint> jointArray = element.getJointArray();
            /// 遍历角度数组进行运动
            for (int i = 0; i < jointArray.size()-1; ++i) {

                this->osgWidget->moveLine(jointArray[i], jointArray[i+1]);

#if 0           /// 每间隔100个插补点绘制一个阴影
                if (i % 100 == 0) {
                    this->osgWidget->drawOneShadow();
                }
#endif

#if 1           /// 绘制tcf工具尖点轨迹
                Eigen::Vector3d p1 = ifkinematicsHandle->getPositionInWorld(jointArray[i]);
                Eigen::Vector3d p2 = ifkinematicsHandle->getPositionInWorld(jointArray[i+1]);
                this->osgWidget->drawLine(p1, p2);
#endif
            }


        /// 如果轨迹是圆弧
        } else if (element.getPathElementType() == MoveArc) {

            displayOnTextEdit(QString(QStringLiteral("--------[圆弧]--------")));
            Point begin = element.getBeginPoint();
            Point mid = element.getMidPoint();
            Point end = element.getEndPoint();
            displayOnTextEdit(QString(QStringLiteral("起点坐标:(%1, %2, %3)")).arg(begin.x).arg(begin.y).arg(begin.z));
            displayOnTextEdit(QString(QStringLiteral("中点坐标:(%1, %2, %3)")).arg(mid.x).arg(mid.y).arg(mid.z));
            displayOnTextEdit(QString(QStringLiteral("终点坐标:(%1, %2, %3)")).arg(end.x).arg(end.y).arg(end.z));

            /// 直线轨迹开始前先绘制一个阴影
            this->osgWidget->drawOneShadow();

            /// 插补获取各轴角度数组
            std::vector<Joint> jointArray = element.getJointArray();
            /// 遍历角度数组进行运动
            for (int i = 0; i < jointArray.size()-1; ++i) {

                this->osgWidget->moveLine(jointArray[i], jointArray[i+1]);

#if 0           /// 每间隔100个插补点绘制一个阴影
                if (i % 100 == 0) {
                    this->osgWidget->drawOneShadow();
                }
#endif

#if 1           /// 绘制tcf工具尖点轨迹
                Eigen::Vector3d p1 = ifkinematicsHandle->getPositionInWorld(jointArray[i]);
                Eigen::Vector3d p2 = ifkinematicsHandle->getPositionInWorld(jointArray[i+1]);
                this->osgWidget->drawLine(p1, p2);
#endif
            }

        }

    }

    displayOnTextEdit(QStringLiteral("=============[结束运行]================="));

}



/* 清除所有轨迹槽函数 **/
void RobotSimulateWidget::clearPathArray()
{

    displayOnTextEdit(QStringLiteral("清除所有轨迹"));

    /// 清除轨迹数组缓存
    this->pathElementArray.clear();

    /// 清除轨迹初始姿态的阴影
    this->osgWidget->removeShadow();

    /// 清除轨迹线
    this->osgWidget->removePath();

}



/* 在textEdit上显示内容
 * @info 要显示的信息字符串
 **/
void RobotSimulateWidget::displayOnTextEdit(const QString& info) const
{

    static int index = 0;
    index++;
    QString str = QString("[%1] %2").arg(index).arg(info);
    this->textEdit->append(str);

}


/****************************以下槽函数**************************************/
/* 加载工件 */
void RobotSimulateWidget::loadObject()
{
    this->fileName = QFileDialog::getOpenFileName(this, QStringLiteral("打开"), "./", QStringLiteral("机器人模型文件(*.stl *osg)"));
    if (fileName.isEmpty()) return;

    QString backName = fileName.right(3);
    if (backName == QStringLiteral("osg") || backName == QStringLiteral("STL") || backName == QStringLiteral("slt")) {
        displayOnTextEdit(QStringLiteral("加载工件中..."));
        osgWidget->addObject(fileName);
        displayOnTextEdit(QString(QStringLiteral("加载完成: %1")).arg(fileName));
    } else {
        displayOnTextEdit(QStringLiteral("未知格式的文件: %1").arg(fileName.right(3)));
        return;
    }

    simulateControlWidget->insertObjectItem(fileName);
}


/* 加载关节 */
void RobotSimulateWidget::loadJoint()
{
#if 0
    this->fileName = QFileDialog::getOpenFileName(this, QStringLiteral("打开"), "./", QStringLiteral("机器人模型文件(*.stl *osg)"));
    if (fileName.isEmpty()) return;

    QString backName = fileName.right(3);
    if (backName == "osg" || backName == "STL" || backName == "slt") {
        displayOnTextEdit(QStringLiteral("加载模型中..."));
        osgWidget->addjoint(fileName);
        displayOnTextEdit(QString(QStringLiteral("加载完成: %1")).arg(fileName));
    } else {
        displayOnTextEdit(QStringLiteral("未知格式的文件: %1").arg(fileName.right(3)));
        return;
    }

    simulateControlWidget->insertJointItem(fileName);
#else

    QStringList pathList = QFileDialog::getOpenFileNames(this, QStringLiteral("打开"), "./", QStringLiteral("机器人模型文件(*.stl *osg)"));

    displayOnTextEdit(QStringLiteral("加载模型中..."));

    for (auto filePath : pathList) {
        QString backName = filePath.right(3);
        if (backName == QStringLiteral("osg") || backName == QStringLiteral("STL") || backName == QStringLiteral("slt")) {
            osgWidget->addjoint(filePath);
        } else {
            displayOnTextEdit(QStringLiteral("未知格式的文件: %1").arg(filePath.right(3)));
            return;
        }
        simulateControlWidget->insertJointItem(filePath);
    }

    simulateControlWidget->setRobotParameterButton->click();
#endif
}


/* 移除模型槽函数 */
void RobotSimulateWidget::removeJoint()
{
    if (osgWidget->removeJoint()) {
        simulateControlWidget->deleteJointItem();
        displayOnTextEdit(QString(QStringLiteral("移除模型: %1")).arg(fileName));
    } else {
        displayOnTextEdit(QStringLiteral("没有需要移除的模型"));
    }
}


void RobotSimulateWidget::removeObject()
{
    if (osgWidget->removeObject()) {
        simulateControlWidget->deleteObjectItem();
        displayOnTextEdit(QString(QStringLiteral("移除工件: %1")).arg(fileName));
    } else {
        displayOnTextEdit(QStringLiteral("没有需要移除的工件"));
    }
}



/** 基座旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::BaseRotate(int angle)
{

    if(osgWidget->rotateBySelfToTargetAngle(BASE, angle)) {

        displayCurrentPositionOnTextEdit();

    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}


/** J1旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J1Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[0];

    if(osgWidget->rotateBySelfToTargetAngle(J1, angle)) {

        displayCurrentPositionOnTextEdit();

    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}



/** J2旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J2Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[1];

    if(osgWidget->rotateBySelfToTargetAngle(J2, angle)) {

        displayCurrentPositionOnTextEdit();


    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}



/** J3旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J3Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[2];

    if(osgWidget->rotateBySelfToTargetAngle(J3, angle)) {

        displayCurrentPositionOnTextEdit();


    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}



/** J4旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J4Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[3];

    if(osgWidget->rotateBySelfToTargetAngle(J4, angle)) {

        displayCurrentPositionOnTextEdit();


    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}



/** J5旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J5Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[4];

    if(osgWidget->rotateBySelfToTargetAngle(J5, angle)) {

        displayCurrentPositionOnTextEdit();


    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }
}



/** J6旋转槽函数
 * @angle 基座要旋转到的目标角度值
 */
void RobotSimulateWidget::J6Rotate(int angle)
{

    /// 获取各轴起始θ角
    QVector<float> theta;
    this->ifkinematicsHandle->getRobotTheta(theta);

    /// 计算实际目标角度值
    angle -= theta[5];

    if(osgWidget->rotateBySelfToTargetAngle(J6, angle)) {

        displayCurrentPositionOnTextEdit();


    } else {

        displayOnTextEdit(QStringLiteral("旋转失败"));

    }

}



/** 在textEdit上显示工具末端坐标值 */
void RobotSimulateWidget::displayCurrentPositionOnTextEdit()
{
    /// 获取各关节角度值
    Joint J = simulateControlWidget->getCurJointsAngle();

    /// 计算末端执行机构的坐标点
    Eigen::Vector3d position = ifkinematicsHandle->getPositionInWorld(J);

#if 0

    displayOnTextEdit(QString(QStringLiteral("各关节角度值: %1 %2 %3 %4 %5 %6"))
                      .arg(J[0]).arg(J[1]).arg(J[2]).arg(J[3]).arg(J[4]).arg(J[5]));

#else

//    displayOnTextEdit(QString(QStringLiteral("工具末端坐标: %1 %2 %3"))
//                        .arg(position[0]).arg(position[1]).arg(position[2]));

#endif

}




/** 通过弹窗设置机器人DH参数槽函数 */
void RobotSimulateWidget::setRobotParameter()
{

    dialog->exec();

    /// 获取机器人参数
    double d1 = 0, a1 = 0, a2 = 0, a3 = 0, d4 = 0;
    QVector<float> Joint_MIN, Joint_MAX;

    this->ifkinematicsHandle->getRobotParameter(d1, a1, a2, a3, d4);
    this->ifkinematicsHandle->getRobotJointLimit(Joint_MIN, Joint_MAX);
    Eigen::Matrix4d tcf = this->ifkinematicsHandle->getTCFMatrix();


    /// 打印到textEdit上
    displayOnTextEdit(QString(QStringLiteral("设置机器人参数:\n d1: %1\n a1: %2\n a2: %3\n a3: %4\n d4: %5"))
                      .arg(d1).arg(a1).arg(a2).arg(a3).arg(d4));

    displayOnTextEdit(QStringLiteral("设置机器人各轴极限角度:"));

    for (int i = 0; i < Joint_MIN.size(); ++i) {

        displayOnTextEdit(QString(QStringLiteral("J%1: %2° ~ %3°")).arg(i+1).arg(Joint_MIN[i]).arg(Joint_MAX[i]));

    }

}




void RobotSimulateWidget::customObject()
{

    displayOnTextEdit(QStringLiteral("自定义工件"));
    customObjectDialog->exec();

}


void RobotSimulateWidget::buildBox(std::vector<float> boxInfo)
{
    displayOnTextEdit(QString(QStringLiteral("自定义矩形工件长宽高: %1mm, %2mm, %3mm"))
                      .arg(boxInfo[0]).arg(boxInfo[1]).arg(boxInfo[2]));
    displayOnTextEdit(QStringLiteral("生成中..."));

    std::string objectName = this->osgWidget->buildBox(boxInfo);

    if (objectName != "") {
        displayOnTextEdit(QStringLiteral("自定义工件完成"));
        /// 将新自定义的工件名称添加到控制界面工件列表中
        this->simulateControlWidget->insertObjectItem(QString::fromStdString(objectName));
    } else {
        displayOnTextEdit(QStringLiteral("自定义工件完成失败: 矩形参数个数不符合"));
    }

}


void RobotSimulateWidget::buildBall(float radius)
{
    displayOnTextEdit(QString(QStringLiteral("自定义球体工件半径: %1mm")).arg(radius));
    displayOnTextEdit(QStringLiteral("生成中..."));

    std::string objectName = this->osgWidget->buildBall(radius);

    if (objectName != "") {
        displayOnTextEdit(QStringLiteral("自定义工件完成"));
        /// 将新自定义的工件名称添加到控制界面工件列表中
        this->simulateControlWidget->insertObjectItem(QString::fromStdString(objectName));
    } else {
        displayOnTextEdit(QStringLiteral("自定义工件完成失败: 矩形参数个数不符合"));
    }
}


void RobotSimulateWidget::buildCylinder(std::vector<float> cylinderInfo)
{
    displayOnTextEdit(QString(QStringLiteral("自定义圆柱工件半径高度: %1mm, %2mm")).arg(cylinderInfo[0]).arg(cylinderInfo[1]));
    displayOnTextEdit(QStringLiteral("生成中..."));


    std::string objectName = this->osgWidget->buildCylinder(cylinderInfo);

    if (objectName != "") {
        displayOnTextEdit(QStringLiteral("自定义工件完成"));
        /// 将新自定义的工件名称添加到控制界面工件列表中
        this->simulateControlWidget->insertObjectItem(QString::fromStdString(objectName));
    } else {
        displayOnTextEdit(QStringLiteral("自定义工件完成失败: 矩形参数个数不符合"));
    }

}



void RobotSimulateWidget::assembleTool()
{
    QString toolName = osgWidget->assembleTool();
    if (toolName != "") {
        displayOnTextEdit(QStringLiteral("成功装配工具: ") + toolName);
    } else {
        displayOnTextEdit(QStringLiteral("请在工件列表中选择待装配的工件"));
    }
}



void RobotSimulateWidget::hideAxisMode()
{
    if (simulateControlWidget->getRadioButton()->isChecked()) {
        if (osgWidget->setHideAxisMode(true)) {
            displayOnTextEdit(QStringLiteral("显示坐标系"));
        } else {
            displayOnTextEdit(QStringLiteral("请选择要显示坐标系的工件"));
        }
    } else {
        if (osgWidget->setHideAxisMode(false)) {
            displayOnTextEdit(QStringLiteral("隐藏坐标系"));
        } else {
            displayOnTextEdit(QStringLiteral("请选择要隐藏坐标系的工件"));
        }
    }
}

